package com.lejos.carcontrol.version1;

import lejos.nxt.LCD;

public class CarControl {

  private static final int TURN_LEFT = 0;
  private static final int TURN_RIGHT = 1;
  private static final int FOWARD = 2;
  private static final int BACKWARD = 3;
  private static final int STOP = 4;
  private static final int CMD_SUCCESS = 5;
  private static final int CMD_FAILED = 6;
  private static final int DISCONNECT = 7;
  private static final int SPEED_REQ = 8;
  private static final int SENSORROTATE = 9;
  private static final int DISTANCEREQ = 10;
  private AndrNxtConnection anConnection;
  private Car car;
  /**
   * @param args
   */
  public static void main(String[] args) {
    // TODO Auto-generated method stub
    CarControl carControl = new CarControl();
    carControl.init();
    
    while(carControl.commAndroid());
  }
  public CarControl(){
    anConnection = new AndrNxtConnection();
    car = new Car();
  }
  public void init(){
    anConnection.Connect();
  }
  
  public boolean commAndroid(){
    LCD.drawString("Into read", 0, 3);
    byte[] readData = anConnection.ReadBytes();
    
    if(readData.length > 0){
      LCD.drawString("ReadData "+readData[0], 0, 4);
      int result = handleCommand(readData);
      if(result != DISCONNECT){
        anConnection.WriteBytes(new byte[]{
            (byte)(result/10/10%10),(byte)(result/10%10),(byte)(result%10)});
        return true;
      }
      else{
        //anConnection.WriteBytes(new byte[]{CMD_SUCCESS});
        anConnection.Disconnect();
        return false;
      }
    }
    else{
      anConnection.Disconnect();
      return false;
    }
  }
  public int handleCommand(byte[] cmd_id){
    switch(cmd_id[0]){
    case TURN_LEFT:
      car.turnLeft((int)cmd_id[1]);
      break;
    case TURN_RIGHT:
      car.turnRight((int)cmd_id[1]);
      break;
    case FOWARD:
      car.Forard((int)cmd_id[1]);
      break;
    case BACKWARD:
      car.Back((int)cmd_id[1]);
      break;
    case STOP:
      car.Stop();
      break;
    case SENSORROTATE:
      car.sensorRotate((int)cmd_id[1]);
      break;
    case DISTANCEREQ:
      int dis = car.getDistance();
      LCD.drawInt(dis, 0, 5);
      return dis;
    case DISCONNECT:
      return DISCONNECT;
    default:
        return CMD_FAILED;
    }
    return CMD_SUCCESS;
  }
}
